load('736858759126.mat');
ranges = distances(:,2);

% lx = -1509:0.1:-1505;
% ly = -2020:1:-1990;
% lz = -840:0.05:-839;
% lx = -1800:4:-1200;
% ly = -2300:4:-2000;
% lz  = 0;
lx = -1500:3:-1200;
ly = -2300:5:-2000;
lz  = -500:10:500;
minres = 1e6;
index  = zeros(1,3);
maps=zeros(length(lx),length(ly),length(lz));
for x = 1:length(lx)
    for y = 1:length(ly)
        for z=1:length(lz)
            p = [lx(x),ly(y),lz(z)];
            maps(x,y,z) =norm( rangingEquation(p,bases,ranges));
            if(maps(x,y,z)<minres )
                index = [x,y,z];
                minres = maps(x,y,z);
            end
        end
    end
end
minres
[lx(index(1)),ly(index(2)),lz(index(3))]
% P0=[ -1507.4 ,  -2020 ,  10000];
P0=[-1440     ,  -2000    ,    -840];
options=optimset('MaxFunEvals',1e20,'TolFun',1e-18,'MaxIter',300,'display','iter');
P = fsolve(@(x)rangingEquation(x,bases,ranges),P0,options);
